/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2010, Heriot-Watt University, UK.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Heriot-Watt University nor the names of 
*     its contributors may be used to endorse or promote products 
*     derived from this software without specific prior written
*     permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
*  Author: Joel Cartwright
*
*********************************************************************/

#include <iostream>

using namespace std;

#include <ros/ros.h>
#include <nodelet/nodelet.h>
#include <pluginlib/class_list_macros.h>
#include <nav_msgs/Odometry.h>

namespace transport_test
{

class Receiver : public nodelet::Nodelet
{
public:
	static const size_t MAX_NUM_MSGS = 100000;

	Receiver() {}

private:
	virtual void onInit()
	{
		ros::NodeHandle& private_nh = getMTNodeHandle();

		std::vector<std::string> argv = getMyArgv();

		if (argv.size() != 2)  {
			NODELET_ERROR_STREAM("Missing arguments.");
			NODELET_ERROR_STREAM("Expected arguments: transportType testName");
			ros::shutdown();
			return;
		}

		std::string transportType = argv.at(0);
		testName_ = argv.at(1);

		NODELET_INFO_STREAM("Transport type: " << transportType);
		NODELET_INFO_STREAM("Test name: " << testName_);

		ros::TransportHints hints;

		if (transportType == "tcp") {
			hints = ros::TransportHints();
		}
		else if (transportType == "tcpNoDelay") {
			hints = ros::TransportHints().tcpNoDelay();
		}
		else if (transportType == "udp") {
			hints = ros::TransportHints().udp();
		}
		else {
			NODELET_ERROR_STREAM("Unsupported transport type: " << transportType);
			ros::shutdown();
			return;
		}

		numMsgsReceived_ = 0;
		nextMsgNum_ = 1;
		numMsgsLost_ = 0;

		testFinished_ = false;

		memset(tofList_, 0, MAX_NUM_MSGS * sizeof(double));

		sub_ = private_nh.subscribe<nav_msgs::Odometry>("/transport_test", 1, &Receiver::callback, this, hints);
	}

	void callback(const nav_msgs::OdometryConstPtr& msg)
	{
		ros::Time timeNow = ros::Time::now();

		if (testFinished_) {
			NODELET_WARN_STREAM("Ignoring callback after test finished.");
			return;
		}

		++numMsgsReceived_;

		assert(numMsgsReceived_ <= MAX_NUM_MSGS);

		unsigned totalExpected = (unsigned)msg->pose.pose.position.y;
		unsigned msgNum = (unsigned)msg->pose.pose.position.z;

		double tof = timeNow.toSec() - msg->header.stamp.toSec();
		tofList_[numMsgsReceived_ - 1] = tof;

		//NODELET_INFO_STREAM(numMsgsReceived_ << " Received msg containing num " << msgNum << " of " << totalExpected << ", tof " << tof);

		if (msgNum != nextMsgNum_) {
			unsigned lostNow = msgNum - nextMsgNum_;
			//NODELET_WARN_STREAM(numMsgsReceived_ << " Received msg num " << msgNum << " not equal to next num " << nextMsgNum_ << ", lost: " << lostNow);
			numMsgsLost_ += lostNow;
		}
		nextMsgNum_ = msgNum + 1;

		if (msgNum == totalExpected) {
			NODELET_INFO_STREAM(numMsgsReceived_ << " Received last msg.");
			testFinished_ = true;

			NODELET_INFO_STREAM("-----------------------------------------");

			cerr << testName_ << " = [";
			for (unsigned i=0; i<numMsgsReceived_; ++i) {
				cerr << " " << tofList_[i];
			}
			cerr << " ];" << endl << endl;

			if (numMsgsLost_ == 0) {
				NODELET_INFO_STREAM("No msgs lost.");
			}
			else {
				NODELET_WARN_STREAM("Total lost msgs: " << numMsgsLost_ << " / " << totalExpected);
			}

			// Calculate time of flight mean.
			double tofSum = 0;
			for (unsigned i=0; i<numMsgsReceived_; ++i) {
				tofSum += tofList_[i];
			}
			double tofMean = tofSum / numMsgsReceived_;
			// Calculate time of flight variance.
			double tofVarSum = 0;
			for (unsigned i=0; i<numMsgsReceived_; ++i) {
				tofVarSum += (tofList_[i] - tofMean) * (tofList_[i] - tofMean);
			}
			double tofVariance = tofVarSum / numMsgsReceived_;

			NODELET_INFO_STREAM("Time of flight mean " << tofMean << ", variance " << tofVariance);

			ros::shutdown();
		}
	}

	unsigned numMsgsReceived_;
	unsigned nextMsgNum_;
	unsigned numMsgsLost_;
	bool testFinished_;
	std::string testName_;

	double tofList_[MAX_NUM_MSGS];

	ros::Subscriber sub_;
};

PLUGINLIB_DECLARE_CLASS(transport_test, Receiver, transport_test::Receiver, nodelet::Nodelet);
}
